﻿using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace Engine.Math
{
    public static class MathHelperEx
    {
        private const double Tolerance = 0.000001f;

        public static  bool AreAngleEqual(double angleA,double angleB,double tolerance)
        {
            double difference = ((angleB - angleA + MathHelper.Pi)% MathHelper.TwoPi) - MathHelper.Pi;

            if (System.Math.Abs(difference) < tolerance)
                return true;

            return false;
        }
        public static bool PointInsidePoly(
            Vector3 vA, Vector3 vB, Vector3 vC, Vector3 p)
        {
            double sumAngles = 0;
            Vector3[] points = new[] {vA, vB, vC};
            for (int i = 0; i < 3; i++)
            {
                Vector3 segment1 = points[i] - p;
                Vector3 segment2 = points[(i + 1)%3] - p;
                if (segment1.Length()*segment2.Length() <= Tolerance)
                {
                    sumAngles = MathHelper.TwoPi;
                    break;
                }
                double cosAngle = (Vector3.Dot(segment1, segment2))/(segment1.Length()*segment2.Length());
                sumAngles += System.Math.Acos(cosAngle);
            }
            if ((sumAngles <= (MathHelper.TwoPi + Tolerance)) && (sumAngles >= (MathHelper.TwoPi - Tolerance)))
                return true;
            return false;
        }

        public static void RayIntersectsTriangle(ref Ray ray,
                                                 ref Vector3 vertex1,
                                                 ref Vector3 vertex2,
                                                 ref Vector3 vertex3, out float? result)
        {
            // Compute vectors along two edges of the triangle.
            Vector3 edge1, edge2;

            Vector3.Subtract(ref vertex2, ref vertex1, out edge1);
            Vector3.Subtract(ref vertex3, ref vertex1, out edge2);

            // Compute the determinant.
            Vector3 directionCrossEdge2;
            Vector3.Cross(ref ray.Direction, ref edge2, out directionCrossEdge2);

            float determinant;
            Vector3.Dot(ref edge1, ref directionCrossEdge2, out determinant);

            // If the ray is parallel to the triangle plane, there is no collision.
            if (determinant > -float.Epsilon && determinant < float.Epsilon)
            {
                result = null;
                return;
            }

            float inverseDeterminant = 1.0f/determinant;

            // Calculate the U parameter of the intersection point.
            Vector3 distanceVector;
            Vector3.Subtract(ref ray.Position, ref vertex1, out distanceVector);

            float triangleU;
            Vector3.Dot(ref distanceVector, ref directionCrossEdge2, out triangleU);
            triangleU *= inverseDeterminant;

            // Make sure it is inside the triangle.
            if (triangleU < 0 || triangleU > 1)
            {
                result = null;
                return;
            }

            // Calculate the V parameter of the intersection point.
            Vector3 distanceCrossEdge1;
            Vector3.Cross(ref distanceVector, ref edge1, out distanceCrossEdge1);

            float triangleV;
            Vector3.Dot(ref ray.Direction, ref distanceCrossEdge1, out triangleV);
            triangleV *= inverseDeterminant;

            // Make sure it is inside the triangle.
            if (triangleV < 0 || triangleU + triangleV > 1)
            {
                result = null;
                return;
            }

            // Compute the distance along the ray to the triangle.
            float rayDistance;
            Vector3.Dot(ref edge2, ref distanceCrossEdge1, out rayDistance);
            rayDistance *= inverseDeterminant;

            // Is the triangle behind the ray origin?
            if (rayDistance < 0)
            {
                result = null;
                return;
            }

            result = rayDistance;
        }

        public static Rectangle RotateRectangle(Rectangle rectangle, Vector2 origin, float angle)
        {
            Matrix translation1 = Matrix.CreateTranslation(-origin.X, -origin.Y, 0);
            Matrix translation2 = Matrix.CreateTranslation(origin.X, origin.Y, 0);
            Matrix rotationMatrix = Matrix.CreateRotationZ(angle);
            Vector2 vec1 = new Vector2(rectangle.Left, rectangle.Top);
            Vector2 vec2 = new Vector2(rectangle.Right, rectangle.Top);
            Vector2 vec3 = new Vector2(rectangle.Right, rectangle.Bottom);
            Vector2 vec4 = new Vector2(rectangle.Left, rectangle.Bottom);
            vec1 = Vector2.Transform(vec1, translation1*rotationMatrix*translation2);
            vec2 = Vector2.Transform(vec2, translation1*rotationMatrix*translation2);
            vec3 = Vector2.Transform(vec3, translation1*rotationMatrix*translation2);
            vec4 = Vector2.Transform(vec4, translation1*rotationMatrix*translation2);
            Rectangle newRectangle = new Rectangle();
            newRectangle.X = (int) System.Math.Min(System.Math.Min(System.Math.Min(vec1.X, vec2.X), vec3.X), vec4.X);
            newRectangle.Y = (int) System.Math.Min(System.Math.Min(System.Math.Min(vec1.Y, vec2.Y), vec3.Y), vec4.Y);
            newRectangle.Width =
                (int) System.Math.Max(System.Math.Max(System.Math.Max(vec1.X, vec2.X), vec3.X), vec4.X) - newRectangle.X;
            newRectangle.Height =
                (int) System.Math.Max(System.Math.Max(System.Math.Max(vec1.Y, vec2.Y), vec3.Y), vec4.Y) - newRectangle.Y;
            return newRectangle;

        }
        public static double GetSignedAngleBetweenVectors(Vector2 vec1, Vector2 vec2)
        {
            if (vec1 == vec2) return 0;
            vec1.Normalize();
            vec2.Normalize();
            float cosAngle = Vector2.Dot(vec1, vec2);
            double angle = System.Math.Acos(cosAngle);

            if (vec2.X < vec1.X)
                return -angle;

            return angle;
        }
        public static double GetAngleBetweenVectors(Vector2 vec1, Vector2 vec2)
        {
            if (vec1 == vec2) return 0;
            vec1.Normalize();
            vec2.Normalize();
            float cosAngle = Vector2.Dot(vec1, vec2);
            double angle = System.Math.Acos(cosAngle);

            if (vec2.X < vec1.X)
                return 2*System.Math.PI - angle;

            return angle;
        }
        public static Ray GetCursorRay(Point point, Viewport viewPort, Matrix projMatrix, Matrix viewMatrix, Matrix worldMatrix)
        {
            Vector3 near =
                viewPort.Unproject(
                    new Vector3(point.X, point.Y, 0), projMatrix,
                    viewMatrix, worldMatrix);
            Vector3 far =
                viewPort.Unproject(
                    new Vector3(point.X, point.Y, 1), projMatrix,
                    viewMatrix, worldMatrix);
            Vector3 direction = far - near;
            direction.Normalize();

            return new Ray(near, direction);
        }
    }
}
